////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "AStar.hpp"

#include <SmegCore/Math/Math.hpp>

#include "../Grid.hpp"

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace ai {
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
AStar::AStar() :
    BasePathfindingAlgorithm()
{
}

////////////////////////////////////////////////////////////
/// Constructor
////////////////////////////////////////////////////////////
AStar::AStar( Grid* _pGrid ) :
    BasePathfindingAlgorithm( _pGrid )
{
}


////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
AStar::~AStar()
{
}

////////////////////////////////////////////////////////////
/// Find a path between start and end
////////////////////////////////////////////////////////////
const std::vector< sf::Vector2f >& AStar::FindPath( const sf::Vector2f& _Start, const sf::Vector2f& _End ) {
    s_PoolNode.ReleaseAll();
    ClearPath();

    sf::Vector2i tmpPoint,
        endPoint;
    int iMapIdx;
    unsigned uNumCells = m_pGrid->GetNumCells();

    m_pGrid->ConvertToGridSpace( (int)_Start.x, (int)_Start.y, tmpPoint );
    m_pGrid->ConvertToGridSpace( (int)_End.x, (int)_End.y, endPoint );

    // If end point is blocked, we can't go
    if ( m_pGrid->IsBlocked( endPoint.x, endPoint.y ) ) {
        return m_LastFoundPath;
    }

    Node* pCurrentNode = s_PoolNode.Next(),
        * pNeighbourNode,
        * pEndNode = s_PoolNode.Next();
    
    pCurrentNode->Setup( tmpPoint.x, tmpPoint.y );
    pEndNode->Setup( endPoint.x, endPoint.y );

    pCurrentNode->F = Heuristic( tmpPoint, endPoint );

    m_DoneNodes.insert( td_PathMap::value_type( Node::UniqueKey( pCurrentNode, uNumCells ), pCurrentNode ) );
    m_OpenSet.push_back( pCurrentNode );
    
    td_PathPriorityQueue::iterator itLess,
        it;
    while ( !m_OpenSet.empty() ) {
        itLess = m_OpenSet.begin();
        for ( it = itLess + 1; it != m_OpenSet.end(); ++it ) {
            if ( **it < **itLess ) {
                itLess = it;
            }
        }
        pCurrentNode = *itLess;
        m_OpenSet.erase( itLess );

        // End of the search we create and return the path
        if ( (*pCurrentNode) == (*pEndNode) ) {
            ReconstructPath( pCurrentNode );
            return m_LastFoundPath;
        }

        m_ClosedMap.insert( td_PathMap::value_type( Node::UniqueKey( pCurrentNode, uNumCells ), pCurrentNode ) );

        // For all node's neighbour
        for ( unsigned i = 0; i < 8; ++i ) {
            tmpPoint.x      = pCurrentNode->X + s_Sides[i].x;
            tmpPoint.y      = pCurrentNode->Y + s_Sides[i].y;

            pNeighbourNode  = s_PoolNode.Next();
            pNeighbourNode->Setup( tmpPoint.x, tmpPoint.y );
            iMapIdx         = Node::UniqueKey( pNeighbourNode, uNumCells );
            
            // If the point is blocked 
            // OR
            // If the point is already in the closed set
            // OR
            // If the point is a diagonal and if it isn't blocked but the two points in x/y direction are
            // THEN
            // No need to go further
            if ( m_pGrid->IsBlocked( tmpPoint.x, tmpPoint.y ) ||
                (i >= 4 && 
                    !m_pGrid->IsBlocked( tmpPoint.x, tmpPoint.y ) &&
                    m_pGrid->IsBlocked( tmpPoint.x, pCurrentNode->Y ) && 
                    m_pGrid->IsBlocked( pCurrentNode->X, tmpPoint.y )) ||
                m_ClosedMap.count( iMapIdx ) != 0 ) {
                s_PoolNode.Release( pNeighbourNode );
                continue;
            }

            // Is it a straight (i<4) or a diagonal (i>=4) ?
            pNeighbourNode->parent = pCurrentNode;
            pNeighbourNode->G = ( i < 4 ) ? pCurrentNode->G + 1 : pCurrentNode->G + math::SQRT2;

            if ( (*pNeighbourNode) == (*pEndNode) ) {
                ReconstructPath( pNeighbourNode );
                return m_LastFoundPath;
            }

            // If this node has already been computed, reprioritize it, don't push it
            td_PathMap::iterator itNeighbour = m_DoneNodes.find( iMapIdx );

            if ( itNeighbour == m_DoneNodes.end() ) {
                pNeighbourNode->F = pNeighbourNode->G + Heuristic( tmpPoint, endPoint );

                // Add it to the open set
                m_OpenSet.push_back( pNeighbourNode );
                m_DoneNodes.insert( td_PathMap::value_type( iMapIdx, pNeighbourNode ) );
            } else if ( pNeighbourNode->G < itNeighbour->second->G ) {
                // Reprioritize existing node
                itNeighbour->second->parent = pNeighbourNode->parent;
                itNeighbour->second->G      = pNeighbourNode->G;
                itNeighbour->second->F      = pNeighbourNode->G + Heuristic( tmpPoint, endPoint );

                s_PoolNode.Release( pNeighbourNode );
            } else {
                s_PoolNode.Release( pNeighbourNode );
            }
        }
    }

    return m_LastFoundPath;
}

#ifdef _DEBUG
bool AStar::DebugStartPath( const sf::Vector2f& _Start, const sf::Vector2f& _End ) { 
    s_PoolNode.ReleaseAll();
    ClearPath();

    sf::Vector2i tmpPoint,
        endPoint;

    m_pGrid->ConvertToGridSpace( (int)_Start.x, (int)_Start.y, tmpPoint );
    m_pGrid->ConvertToGridSpace( (int)_End.x, (int)_End.y, endPoint );

    // If end point is blocked, we can't go
    if ( m_pGrid->IsBlocked( endPoint.x, endPoint.y ) ) {
        return false;
    }

    Node* pCurrentNode = s_PoolNode.Next(),
        * pEndNode = s_PoolNode.Next();
    
    pCurrentNode->Setup( tmpPoint.x, tmpPoint.y );
    pEndNode->Setup( endPoint.x, endPoint.y );

    pCurrentNode->F = Heuristic( tmpPoint, endPoint );

    m_DoneNodes.insert( td_PathMap::value_type( Node::UniqueKey( pCurrentNode, m_pGrid->GetNumCells() ), pCurrentNode ) );
    m_OpenSet.push_back( pCurrentNode );

    return true;
}

bool AStar::DebugStepPath( const sf::Vector2f& _Start, const sf::Vector2f& _End ) {
    sf::Vector2i tmpPoint,
        endPoint;
    int iMapIdx;
    unsigned uNumCells = m_pGrid->GetNumCells();
    
    m_pGrid->ConvertToGridSpace( (int)_End.x, (int)_End.y, endPoint );
    Node* pCurrentNode,
        * pNeighbourNode,
        * pEndNode = s_PoolNode.Next();

    pEndNode->Setup( endPoint.x, endPoint.y );

    if ( m_ClosedMap.find( Node::UniqueKey( pEndNode, uNumCells ) ) != m_ClosedMap.end() ) {
        return true;
    }

    td_PathPriorityQueue::iterator itLess,
        it;
    if ( !m_OpenSet.empty() ) {
        itLess = m_OpenSet.begin();
        for ( it = itLess + 1; it != m_OpenSet.end(); ++it ) {
            if ( **it < **itLess ) {
                itLess = it;
            }
        }
        pCurrentNode = *itLess;
        m_OpenSet.erase( itLess );
        
        m_ClosedMap.insert( td_PathMap::value_type( Node::UniqueKey( pCurrentNode, uNumCells ), pCurrentNode ) );

        // End of the search we create and return the path
        if ( (*pCurrentNode) == (*pEndNode) ) {
            ReconstructPath( pCurrentNode );
            return true;
        }

        // For all node's neighbour
        for ( unsigned i = 0; i < 8; ++i ) {
            tmpPoint.x      = pCurrentNode->X + s_Sides[i].x;
            tmpPoint.y      = pCurrentNode->Y + s_Sides[i].y;

            pNeighbourNode  = s_PoolNode.Next();
            pNeighbourNode->Setup( tmpPoint.x, tmpPoint.y );
            iMapIdx         = Node::UniqueKey( pNeighbourNode, uNumCells );
            
            // If the point is blocked 
            // OR
            // If the point is already in the closed set
            // OR
            // If the point is a diagonal and if it isn't blocked but the two points in x/y direction are
            // THEN
            // No need to go further
            if ( m_pGrid->IsBlocked( tmpPoint.x, tmpPoint.y ) ||
                (i >= 4 && 
                    !m_pGrid->IsBlocked( tmpPoint.x, tmpPoint.y ) &&
                    m_pGrid->IsBlocked( tmpPoint.x, pCurrentNode->Y ) && 
                    m_pGrid->IsBlocked( pCurrentNode->X, tmpPoint.y )) ||
                m_ClosedMap.count( iMapIdx ) > 0 ) {
                s_PoolNode.Release( pNeighbourNode );
                continue;
            }

            // Is it a straight (i<4) or a diagonal (i>=4) ?
            pNeighbourNode->parent  = pCurrentNode;
            pNeighbourNode->G       = ( i < 4 ) ? pCurrentNode->G + 1 : pCurrentNode->G + math::SQRT2;

            // If this is the end node, reconstruct the path
            if ( (*pNeighbourNode) == (*pEndNode) ) {
                m_ClosedMap.insert( td_PathMap::value_type( Node::UniqueKey( pNeighbourNode, uNumCells ), pNeighbourNode ) );
                ReconstructPath( pNeighbourNode );
                return true;
            }

            // If this node has already been computed, reprioritize it, don't push it
            td_PathMap::iterator itNeighbour = m_DoneNodes.find( iMapIdx );

            if ( itNeighbour == m_DoneNodes.end() ) {
                pNeighbourNode->F = pNeighbourNode->G + Heuristic( tmpPoint, endPoint );

                // Add it to the open set
                m_OpenSet.push_back( pNeighbourNode );
                m_DoneNodes.insert( td_PathMap::value_type( iMapIdx, pNeighbourNode ) );
            } else if ( pNeighbourNode->G < itNeighbour->second->G ) {
                // Reprioritize existing node
                itNeighbour->second->G      = pNeighbourNode->G;
                itNeighbour->second->F      = pNeighbourNode->G + Heuristic( tmpPoint, endPoint );
                itNeighbour->second->parent = pNeighbourNode->parent;

                s_PoolNode.Release( pNeighbourNode );
            } else {
                s_PoolNode.Release( pNeighbourNode );
            }
        }
    }

    s_PoolNode.Release( pEndNode );
    return false;
}
#endif

    } // namespace ai
} // namespace smeg